/**
  ******************************************************************************
  * @file    smooth.c
  * @author  vonku@RM2017
  * @version V1.0
  * @date    2017-7-26 11:29:42
  * @brief   This file provides all the smooth functions.
  ******************************************************************************
  * @attention
  *
  * FILE FOR RM 2017 ONLY
  *
  * Copyright (C), 2017-2027, DJI Robomaster
  ******************************************************************************
  */ 

/* Includes ------------------------------------------------------------------*/
#include    "smooth.h"
#include "DSP28x_Project.h"     // Device Headerfile and Examples Include File

/* Private macro -------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/

SmoothfilterTypeDef strain_filter; //重构角度的滤波器
SmoothfilterTypeDef strain1_filter; //应变1的滤波器
SmoothfilterTypeDef strain2_filter; //应变2的滤波器
SmoothfilterTypeDef strain3_filter; //应变3的滤波器



SmoothfilterintTypeDef current_filter;


float strain_filter_buf[10] = {0};
float strain1_filter_buf[10] = {0};
float strain2_filter_buf[10] = {0};
float strain3_filter_buf[10] = {0};

int current_buf[100] = {0};

/**
  * @brief  Initialization for a smooth filter
  * @note   None
  * @param  my_Filter : a ptr points to a smooth filter struct
                myNum : depth of smooth filter buffer
  * @retval 0 : sucess
  * @author vonku@2017-7-26 11:52:26
  */

static int Smoothfilter_Init_One(SmoothfilterTypeDef *my_Filter, int myNum)
{
    if (myNum < 0)
    {
        my_Filter->num = 0;
        return -1;
    }
    my_Filter->num = myNum;
    my_Filter->sum = 0;

    /* 将动态分配改为了静态分配 */
    if(my_Filter == &strain_filter)
    	my_Filter->databuf = strain_filter_buf;

    if(my_Filter == &strain1_filter)
    	my_Filter->databuf = strain1_filter_buf;

    if(my_Filter == &strain2_filter)
    	my_Filter->databuf = strain2_filter_buf;

    if(my_Filter == &strain3_filter)
    	my_Filter->databuf = strain3_filter_buf;

    if (my_Filter->databuf == NULL)
    {
        return -1; //
    }
    //memset(my_Filter->databuf, 0, my_Filter->num * sizeof(float));
    my_Filter->Front = my_Filter->Rear = 0;
    return 0;
}

static int Smoothfilter_Init_One_Int(SmoothfilterintTypeDef *my_Filter, int myNum)
{
    if (myNum < 0)
    {
        my_Filter->num = 0;
        return -1;
    }
    my_Filter->num = myNum;
    my_Filter->sum = 0;

    my_Filter->databuf = current_buf;

	if (my_Filter->databuf == NULL)
    {
        return -1; //
    }
    //memset(my_Filter->databuf, 0, my_Filter->num * sizeof(int));
    my_Filter->Front = my_Filter->Rear = 0;
    return 0;
}

/**
  * @brief  Initialization for smoothfilter, called by main()
  * @note   None
  * @param  None
  * @retval None
  * @author vonku@2017-7-26 12:22:05
  */
int smoothfilter_init(void)
{

	if(Smoothfilter_Init_One(&strain_filter, 10) != 0)
		return -1;

	if(Smoothfilter_Init_One(&strain1_filter, 10) != 0)
		return -1;

	if(Smoothfilter_Init_One(&strain2_filter, 10) != 0)
		return -1;

	if(Smoothfilter_Init_One(&strain3_filter, 10) != 0)
		return -1;

	if(Smoothfilter_Init_One_Int(&current_filter, 100) != 0)
		return -1;



	return 0;
}


/**
  * @brief  get smooth filter output
  * @note   None
  * @param  my_Filter : a ptr points to a smooth filter struct
  * @retval filter output, 0 if the buffer have no member
  * @author vonku@2017-7-26 11:52:02
  */
float smoothfilter_get_value(SmoothfilterTypeDef *my_Filter)
{
    if(my_Filter->databuf == NULL)
    {
        return 0;
    }
    if(my_Filter->num == 0)
    {
        return 0;
    }
    my_Filter -> ref = 1.0f*my_Filter->sum/my_Filter->num;
    return my_Filter -> ref;
}

float smoothfilter_get_value_int(SmoothfilterintTypeDef *my_Filter)
{
    if(my_Filter->databuf == NULL)
    {
        return 0;
    }
    if(my_Filter->num == 0)
    {
        return 0;
    }
    my_Filter -> ref = 1.0f*my_Filter->sum/my_Filter->num;
    return my_Filter -> ref;
}


/**
  * @brief  add a new member to smooth filter buffer
  * @note   None
  * @param  my_Filter : a ptr points to a smooth filter struct
                myData : new data of smooth filter buffer
  * @retval 0 : seccess
  * @author vonku@2017-7-26 11:52:23
  */

int smoothfilter_add(SmoothfilterTypeDef *my_Filter, float myData)
{
    int temp_rear = 0, temp_front = 0;
    if (my_Filter == NULL)
    {
        return -1;
    }
    temp_rear = my_Filter->Rear + 1;
    if (temp_rear >= my_Filter->num)
    {
        temp_rear = 0;
    }
    if (temp_rear == my_Filter->Front) //缂撳啿鍖哄凡婊�
    {
        my_Filter->sum -= my_Filter->databuf[temp_rear];
        my_Filter->databuf[temp_rear] = myData;
        my_Filter->sum += my_Filter->databuf[temp_rear];

        my_Filter->Rear = temp_rear;
        temp_front = temp_rear + 1;
        if(temp_front >= my_Filter->num)
        {
            temp_front = 0;
        }
        my_Filter->Front = temp_front;
    }
    else  //
    {
        my_Filter->sum += myData;
        my_Filter->databuf[temp_rear] = myData;
        my_Filter->Rear = temp_rear;
    }

	smoothfilter_get_value(my_Filter);  //update filter output when new member added.
	
    return 0;
}

int smoothfilter_add_int(SmoothfilterintTypeDef *my_Filter, int myData)
{
    int temp_rear = 0, temp_front = 0;
    if (my_Filter == NULL)
    {
        return -1;
    }
    temp_rear = my_Filter->Rear + 1;
    if (temp_rear >= my_Filter->num)
    {
        temp_rear = 0;
    }
    if (temp_rear == my_Filter->Front) //
    {
        my_Filter->sum -= my_Filter->databuf[temp_rear];
        my_Filter->databuf[temp_rear] = myData;
        my_Filter->sum += my_Filter->databuf[temp_rear];

        my_Filter->Rear = temp_rear;
        temp_front = temp_rear + 1;
        if(temp_front >= my_Filter->num)
        {
            temp_front = 0;
        }
        my_Filter->Front = temp_front;
    }
    else  //
    {
        my_Filter->sum += myData;
        my_Filter->databuf[temp_rear] = myData;
        my_Filter->Rear = temp_rear;
    }

    smoothfilter_get_value_int(my_Filter);  //update filter output when new member added.

    return 0;
}





